/*
 * filter.c
 *
 *  Created on: 2021年7月15日
 *      Author: 本地账户
 */
#include "filter.h"
#include <math.h>

double Q_angle = 0.001;//过程噪声协方差
double Q_gryobias = 0.17;//
double P[2][2] = {{1,0},{0,1}};//
double K[2]={0,0};//
double S = 0;//
double x=0,y,z;//
double Qbias = 0;//

double P00_temp ;
double P01_temp ;

void KalmanFilter(double angle,CondVector* X)
{
	x += dt*(X->omega);

  P[0][0] += dt*(dt*P[1][1]-P[0][1]-P[1][0]+Q_angle);
  P[0][1] -= P[1][1]*dt;
  P[1][0] -= P[1][1]*dt;
  P[1][1] +=Q_gryobias*dt;

  //Update Kalman gain

  y = angle - x;
  S = P[0][0]+R;
  K[0] = P[0][0]/S;
  K[1] = P[1][0]/S;

  //fusion to predict
  x +=K[0]*y;
  X->theta = x;
  Qbias +=K[1]*y;

  //update the cov matrix P(??fusion)
  P00_temp = P[0][0];
  P01_temp = P[0][1];
  P[0][0] -= K[0]*P00_temp;
  P[0][1] -= K[0]*P01_temp ;
  P[1][0] -= K[1]*P00_temp ;
  P[1][1] -= K[1]*P01_temp ;
}






